#ifndef EM80_STATUS_HPP
#define EM80_STATUS_HPP

#include "../packet_base.hpp"
#include "../payload_headers.hpp"
#include "../em80_payload_factory.h"
#include "../../v8stdint.h"

#include <memory>

namespace em80_driver
{

class Status : public EM80PayloadBase
{
public:
    Status(const EM80PacketPtr &packet) : EM80PayloadBase(packet, "Status", false, 6) {}

    struct Data
    {
        uint8_t  left_servo_error;
        uint8_t  right_servo_error;
        uint8_t  imu_error;
        uint16_t battery_error;
        uint8_t  ultrasonic_warning;

    };

    struct Code
    {
        // Error Code
        // imu
        static const uint16_t IMU_OFFLINE = 0x0101;

        // left actuator
        static const uint16_t LEFT_ACTUATOR_OFFLINE = 0x0201;
        static const uint16_t LEFT_ACTUATOR_STORAGE_FAULT = 0x0202;
        static const uint16_t LEFT_ACTUATOR_FPGA_FAULT = 0x0203;
        static const uint16_t LEFT_ACTUATOR_ENCODER_FAULT = 0x0204;
        static const uint16_t LEFT_ACTUATOR_OVERCURRENT_FAULT = 0x0205;
        static const uint16_t LEFT_ACTUATOR_OVERHEATED_FAULT = 0x0206;
        static const uint16_t LEFT_MOTOR_OVERHEATED_FAULT = 0x0207;
        static const uint16_t LEFT_ACTUATOR_OVERSPEED_FAULT = 0x0208;
        static const uint16_t LEFT_ACTUATOR_BATTERY_FAULT = 0x0209;
        static const uint16_t LEFT_ACTUATOR_POWERLOSS_FAULT = 0x020A;

        // right actuator
        static const uint16_t RIGHT_ACTUATOR_OFFLINE = 0x0231;
        static const uint16_t RIGHT_ACTUATOR_STORAGE_FAULT = 0x0232;
        static const uint16_t RIGHT_ACTUATOR_FPGA_FAULT = 0x0233;
        static const uint16_t RIGHT_ACTUATOR_ENCODER_FAULT = 0x0234;
        static const uint16_t RIGHT_ACTUATOR_OVERCURRENT_FAULT = 0x0235;
        static const uint16_t RIGHT_ACTUATOR_OVERHEATED_FAULT = 0x0236;
        static const uint16_t RIGHT_MOTOR_OVERHEATED_FAULT = 0x0237;
        static const uint16_t RIGHT_ACTUATOR_OVERSPEED_FAULT = 0x0238;
        static const uint16_t RIGHT_ACTUATOR_BATTERY_FAULT = 0x0239;
        static const uint16_t RIGHT_ACTUATOR_POWERLOSS_FAULT = 0x023A;

        // battery
        static const uint16_t BATTERY_OFFLINE = 0x0301;
        static const uint16_t BATTERY_OVERVOLTAGE = 0x0302;
        static const uint16_t BATTERY_LOWVOLTAGE = 0x0303;
        static const uint16_t BATTERY_OVERCURRENT = 0x0304;

        //Warning Code
        //ultrasonic
        static const uint16_t ULTRASONIC1_OFFLINE = 0x0101;
        static const uint16_t ULTRASONIC2_OFFLINE = 0x0102;
        static const uint16_t ULTRASONIC3_OFFLINE = 0x0103;
        static const uint16_t ULTRASONIC4_OFFLINE = 0x0104;
        static const uint16_t ULTRASONIC5_OFFLINE = 0x0105;
        static const uint16_t ULTRASONIC6_OFFLINE = 0x0106;
        static const uint16_t ULTRASONIC7_OFFLINE = 0x0107;
        static const uint16_t ULTRASONIC8_OFFLINE = 0x0108;
    };

    Data data;

    ~Status() {}

    //not to be used directly by external source
    bool serialize()
    {
        // //TODO : is it directly control the raw data
        // std::shared_ptr<Buffer> byteStream = std::make_shared<Buffer>(packet->payload(), packet->packet().second);
        // buildBytes(static_cast<uint8_t>(Header::Status), *byteStream);
        // buildBytes(static_cast<uint8_t>(length), *byteStream);
        // buildBytes(data.error_code, *byteStream);
        // buildBytes(data.warning_code, *byteStream);


        return true;
    }


    bool deserialize()
    {
        std::vector<uint8_t> byteStream;

        byteStream.assign(packet->payload(), packet->payload() + static_cast<uint32_t>(length) + 2);


        //TODO: check them
        uint8_t header_id, length_packed;
        buildVariable(header_id, byteStream);
        buildVariable(length_packed, byteStream);
        if(header_id != Header::Status) return false;
        if(length_packed != length) return false;

        buildVariable(data.left_servo_error, byteStream);
        buildVariable(data.right_servo_error, byteStream);
        buildVariable(data.imu_error, byteStream);
        buildVariable(data.battery_error, byteStream);
        buildVariable(data.ultrasonic_warning, byteStream);
        return true;
    }
};



} // namespace em80_driver

#endif /* EM80_ERROR_HPP*/
